#include "Motor.h"

uint32_t *LeftRpm;
uint32_t *RightRpm;
uint16_t State_Flag[2]={0,0};
MotorRpm Motor_Rpm;
extern int Test_PID_Vuale;

void Motor_Init(void)
{
	
	// start PWM output
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
	
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
	
	//set motor break release
	Motor_Break(release,release);
	
	//set motor FR
  Motor_FR(forward,forward);
	
	
	//start PWM capture
	HAL_TIM_IC_Start_IT(&htim3,TIM_CHANNEL_1);
	HAL_TIM_IC_Start_IT(&htim3,TIM_CHANNEL_2);

}


//set motor rpm 0-3000  , '+'  means  forward , '-' means  reverse
void Set_Motor_Rpm(int LeftRpm,int RightRpm)
{
	
	Direction LeftDir = (LeftRpm)>0 ? forward : reverse ;
	Direction RighDir = (RightRpm)>0 ? forward : reverse ;
	
	Motor_FR(LeftDir,RighDir);
	
	LeftRpm = (LeftRpm)>0 ? LeftRpm : -LeftRpm ;
	RightRpm = (RightRpm)>0 ? RightRpm : -RightRpm ;
	
	LeftRpm = (LeftRpm)>3000 ? 3000 : (LeftRpm) ;
	RightRpm = (RightRpm)>3000 ? 3000 : (RightRpm) ;
	
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (LeftRpm)/6);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, (RightRpm)/6);
	
	State_Flag[0]=(LeftRpm==0) ? 1 :0 ;
	State_Flag[1]=(RightRpm==0) ? 1 :0;
	
}


//set motor direction
void Motor_FR(Direction LeftDir,Direction RightDir)
{
	GPIO_PinState PIN_State;

	PIN_State = (LeftDir==forward) ? GPIO_PIN_RESET : GPIO_PIN_SET;
	if (HAL_GPIO_ReadPin(Left_Motor_FR_GPIO_Port, Left_Motor_FR_Pin)!=PIN_State)   
  	HAL_GPIO_WritePin(Left_Motor_FR_GPIO_Port, Left_Motor_FR_Pin, PIN_State);
	  
	
	PIN_State = (RightDir==forward) ? GPIO_PIN_RESET : GPIO_PIN_SET;
	if (HAL_GPIO_ReadPin(Left_Motor_FR_GPIO_Port, Right_Motor_FR_Pin)!=PIN_State)  
		HAL_GPIO_WritePin(Right_Motor_FR_GPIO_Port, Right_Motor_FR_Pin, PIN_State);
}

void MotorRpm_Init(void)
{
	Motor_Rpm.LeftCurRpm=0;
	Motor_Rpm.LeftExpRpm=0;
	Motor_Rpm.RightCurRpm=0;
	Motor_Rpm.RightExpRpm=0;
}

void Motor_Break(Break LeftBre,Break RightBre)
{
	GPIO_PinState PIN_State;
	
	PIN_State = (LeftBre==release) ? GPIO_PIN_RESET : GPIO_PIN_SET;
	if (HAL_GPIO_ReadPin(Left_Motor_Break_GPIO_Port, Left_Motor_Break_Pin)!=PIN_State)   
  	HAL_GPIO_WritePin(Left_Motor_Break_GPIO_Port, Left_Motor_Break_Pin, PIN_State);
	if (LeftBre==hold)
	{
		Motor_Rpm.LeftExpRpm=0;
		Set_Motor_Rpm(0,Motor_Rpm.RightExpRpm);
	}
	
	PIN_State = (RightBre==release) ? GPIO_PIN_RESET : GPIO_PIN_SET;
	if (HAL_GPIO_ReadPin(Right_Motor_Break_GPIO_Port, Right_Motor_Break_Pin)!=PIN_State)   
  	HAL_GPIO_WritePin(Right_Motor_Break_GPIO_Port, Right_Motor_Break_Pin, PIN_State);
	if (RightBre==hold)
	{
		Motor_Rpm.RightExpRpm=0;
		Set_Motor_Rpm(Motor_Rpm.LeftExpRpm,0);
	}
	
}
